/*============================================================================*/
/*                        Tortoise Team			                              */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:         %APP_Algorithm.c%
* Instance:         RPL_1
* %version:         1.1 %
* %created_by:      Oscar Rodea Aragon %
* %date_created:    Friday Oct 26 17:00:01 2012 %
*=============================================================================*/
/* DESCRIPTION : C source file for the algorithm	                          */
/*============================================================================*/
/* FUNCTION COMMENT : Contains the definitions of the functions declared in	  */	
/*	APP_Algorithm.h		                                                  */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 26/05/2012  |                               | Oscar Rodea   	  */
/* Integration under Continuus CM                                             */
/*============================================================================*/

/* Includes */
/* -------- */
#include "stdtypedef.h"
#include "APP_Algorithm.h"
#include "APP_Linear_Camera.h"
#include "APP_FrontWheels.h"
#include "HAL_DcMotor.h"

/* Functions macros, constants, types and datas         */
/* ---------------------------------------------------- */
extern T_UBYTE ub_Black_Strip_Center;

/*==================================================*/ 
/* Definition of constants                          */
/*==================================================*/ 


/*======================================================*/ 
/* Definition of RAM variables                          */
/*======================================================*/ 
/* BYTE RAM variables */


/*======================================================*/ 
/* close variable declaration sections                  */
/*======================================================*/ 

/* Private defines */



/* Private functions prototypes */
/* ---------------------------- */

/* Exported functions prototypes */
/* ----------------------------- */
void vfnFirstVersion(void);

/* Inline functions */
/* ---------------- */


/* Private functions */
/* ----------------- */


/* Exported functions */
/* ------------------ */

/**************************************************************
 *  Name                 :	vfnCalibrate_FrontWheels.
 *  Description          :	Calibrate the front wheels of the car.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
void vfnFirstVersion(void)
{	
	T_UBYTE ub_center_temp;
	
	T_UBYTE CONST_center = 64;
	T_UBYTE CONST_maxPower = 80;
	T_UBYTE CONST_factor_int = 1;
	T_UBYTE CONST_factor_ext = 4;
	T_UBYTE ub_difference = 0;
	
	ub_center_temp = ub_Black_Strip_Center; //Values from 0 to 127
	
	//vfnSet_DcMotor(50,2);			//two motors the same speed
	
	if(ub_center_temp < 10)
	{
		vfnSetPos_FrontWheels(0);		//from 0 to 1023
	}
	else if(ub_center_temp > 117)
	{
		vfnSetPos_FrontWheels(1023);		//from 0 to 1023
	}
	else
	{
		vfnSetPos_FrontWheels(ub_center_temp*8);		//from 0 to 1023
	}
	
	if(ub_IsGoal)
	{
		vfnStop_DcMotor(2);
	}
	
	//Speed changes
	if(CONST_center > ub_center_temp)
	{
		ub_difference = CONST_center - ub_center_temp;
		vfnSet_DcMotor(CONST_maxPower-(ub_difference/CONST_factor_int),1);	//Left
		vfnSet_DcMotor(CONST_maxPower-(ub_difference/CONST_factor_ext),0);	//Right
	}
	else
	{
		ub_difference = ub_center_temp - CONST_center;
		vfnSet_DcMotor(CONST_maxPower-(ub_difference/CONST_factor_int),0);	//Right
		vfnSet_DcMotor(CONST_maxPower-(ub_difference/CONST_factor_ext),1);	//Left
	}
	
	//vfnSet_DcMotor(CONST_maxPower-(ub_difference/CONST_factor),2);	//two motors the same speed
	
}




